/*
 *  This sketch sends data via HTTP GET requests to data.sparkfun.com service.
 *
 *  You need to get streamId and privateKey at data.sparkfun.com and paste them
 *  below. Or just customize this script to talk to other HTTP servers.
 *
 */
#include <ArduinoJson.h>  //arduino json字符串库:https://github.com/bblanchon/ArduinoJson
#include <ESP8266WiFi.h>
#include <Wire.h>
//#include <SCoop.h>
#include <Scheduler.h>

#include "Adafruit_PWMServoDriver.h"

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

WiFiClient client;

typedef unsigned char uchar;
//typedef unsigned int uint;

const char* ssid     = "wifi_SSID";     //路由器名称,只支持2.4G Wifi
const char* password = "wifi_password"; //路由器密码

const char* host = "192.168.123.200";
const int httpPort = 9101;

//#define D0 16
//#define D1 5 // I2C Bus SCL (clock)
//#define D2 4 // I2C Bus SDA (data)
//#define D3 0
//#define D4 2 // Same as "LED_BUILTIN", but inverted logic
//#define D5 14 // SPI Bus SCK (clock)
//#define D6 12 // SPI Bus MISO 
//#define D7 13 // SPI Bus MOSI
//#define D8 15 // SPI Bus SS (CS)
//#define D9 3 // RX0 (Serial console)
//#define D10 1 // TX0 (Serial console)


// you can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 50;   // 60 Hz
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000;
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}

void writeServo(uint8_t n,uint8_t angle){
  double pulse;
  pulse=0.5+angle/90.0;
  setServoPulse(n,pulse);
}



//6个舵机的转动角度数值，舵机编号从机械臂的下边向上分别是0,1,2,3,4,5共计6个舵机，每个舵机的可转成角度为0~180度
//uint8_t是无符号8位整数
uint8_t pwmAngle[6] = {0,0,0,0,0,0};

//pwmSetType接收到的pwm数据类型，
//1.设置到绝对转动角度，
//2.设置到相对转动角度(相对转到角度正为逆时针，负为顺时针方向),
//3.保存当前转动角度为上电角度
//4.设置转动角度时间序列动作
//5.无条件停止当前动作
//6.当前动作运行完成后停止当前动作
//目前暂时想到这么多
int pwmSetType = 0;       

uint8_t pwmLastAngle[6];
void setAngle(){
  for(int i = 0;i < 6;i++){
          if(pwmAngle[i] != pwmLastAngle[i] && pwmAngle[i] >= 0 && pwmAngle[i] <= 180){
            writeServo(i,pwmAngle[i]);
            pwmLastAngle[i] = pwmAngle[i];
          }
        }
}

class LoopTask : public Task {
protected:
    void loop()  {
      delay(1000);

      Serial.print("connecting to ");
      Serial.println(host);
      
      
      Serial.print("Requesting URL: ");
      if(!client.connected()){
        delay(5000);
         if (!client.connect(host, httpPort)) {
            Serial.println("connection failed");
            return;
         }
      }
      
       client.print(String("client is Connect"));
    //   delay(3000);
        
    
      unsigned long timeout = millis();
      while(true){
        if(client.available() == 0){
            while (client.available() == 0) {
              if (millis() - timeout > 5000) {
                Serial.println(">>> Client Timeout !");
                return;
              }
            }
        }else{
          timeout = millis();
          String body = "";
          bool gotResponse = false;
          unsigned long now = millis();
          while (client.available()) {
            char c = client.read();
             body=body+c;
            gotResponse = true;
          }
          if (gotResponse) {
                    //{"type":1,"data":{"1":0,"2":0,"3":0,"4":0,"5":0,"6":0}}
                    //1.设置到绝对转动角度，
      
                    DynamicJsonBuffer jsonBuffer;
                    JsonObject& root = jsonBuffer.parseObject(body);
                    if (root.success()) {
                        
                      if (root.containsKey("type")) {
                        pwmSetType = root["type"].as<int>();
            
                        if(root.containsKey("data"))
                        {
                          JsonObject& ticker = root["data"];
                          pwmAngle[0] = ticker["1"].as<uint8_t>();
                          pwmAngle[1] = ticker["2"].as<uint8_t>();
                          pwmAngle[2] = ticker["3"].as<uint8_t>();
                          pwmAngle[3] = ticker["4"].as<uint8_t>();
                          pwmAngle[4] = ticker["5"].as<uint8_t>();
                          pwmAngle[5] = ticker["6"].as<uint8_t>();
                        }
                        client.print(0);
                      } else {
                        Serial.println("failed to parse JSON");
                        client.print(1);
                      }
                    }
                    Serial.println(body);
                    Serial.println("one step!");
                    setAngle();
           }
        }
      }
    }
    
} loop_task;




void setup() {

  
  Serial.begin(115200);
  delay(10);

  // We start by connecting to a WiFi network

  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  
  /* Explicitly set the ESP8266 to be a WiFi-client, otherwise, it by default,
     would try to act as both a client and an access-point and could cause
     network-issues with your other WiFi-devices on your WiFi-network. */
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");  
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());

  
  if (!client.connect(host, httpPort)) {
    Serial.println("connection failed");
    return;
  }


//init pwm
  Serial.println("16 channel Servo test!");

  pwm.begin();
  pwm.setPWMFreq(50);  // Analog servos run at ~60 Hz updates

//  Scheduler.start(&angleSet_task);
  Scheduler.start(&loop_task);
  Scheduler.begin();
}

void loop() {}

